#ifndef _Filter_H
#define _Filter_H
//#include "zf_common_headfile.h"
#define M_PI_F 3.141592653589793f
typedef struct
{
 float Input_Butter[3];
 float Output_Butter[3];
}Butter_BufferData;

typedef struct
{
  float a[3];
  float b[3];
}Butter_Parameter;

//卡尔曼滤波器参数
typedef struct 
{
    float LastP;//上次估算协方差 初始化值为0.02
    float Now_P;//当前估算协方差 初始化值为0
    float out;//卡尔曼滤波器输出 初始化值为0
    float Kg;//卡尔曼增益 初始化值为0
    float Q;//过程噪声协方差 初始化值为0.001
    float R;//观测噪声协方差 初始化值为0.543
} Kalman_Filter;
extern Kalman_Filter KFP;

//均值滤波参数
#define MEAN_FILTER_NUM 200 //滤波器次数
typedef struct
{
  float Filter[MEAN_FILTER_NUM];//历史值
  //下面是优化滤波需要使用到
  unsigned char NewNumPtr; // 新值指针
  unsigned int count;//计数，用于非连续累计
} Mean_Filter;

//低通滤波参数
#define u8data 0
#define s8data 1
#define u16data 2
#define s16data 3
#define u32data 4
#define s32data 5
#define fdata 6
typedef struct
{
  unsigned char u8Data[4];
  unsigned short u16Data[4];
  unsigned long u32Data[4];
  char s8Data[4];
  short s16Data[4];
  long s32Data[4];
  float fData[4];
} LPF_Filter;

extern Butter_Parameter Butter_80HZ_Parameter_Acce,Butter_60HZ_Parameter_Acce,Butter_51HZ_Parameter_Acce,
                 Butter_30HZ_Parameter_Acce,Butter_20HZ_Parameter_Acce,Butter_15HZ_Parameter_Acce,
                 Butter_10HZ_Parameter_Acce,Butter_5HZ_Parameter_Acce,Butter_2HZ_Parameter_Acce;

extern Butter_BufferData Butter_Buffer_ACC_X,Butter_Buffer_ACC_Y,Butter_Buffer_ACC_Z,
Butter_Buffer_GyroX,Butter_Buffer_GyroY,Butter_Buffer_GyroZ,Butter_Buffer_Carangle;

extern Butter_BufferData Butter_Buffer_speed;
void LPF_1_db(float hz,float time,float in,float *out);
void Butterworth_Parameter_Init(void);
float LPButterworth(float curr_input,Butter_BufferData *Buffer,Butter_Parameter *Parameter);
void set_cutoff_frequency(float sample_frequent, float cutoff_frequent,Butter_Parameter *LPF);

void Kalman_Filter_Init(Kalman_Filter *KFP, float P, float Q, float R);
float KalmanFilter(Kalman_Filter *kfp, float input);

long MeanFilterBetter1(Mean_Filter *Filter, int input);
void LPF(LPF_Filter *Filter, void *nData, unsigned char nType);
#endif
